import time
from collections import deque
from datetime import datetime

from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg

from cmd_all import BasicCmd, GimbalStatus, HeadtrackEulerAngle
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from collections import deque
import numpy as np


import tkinter as tk
from tkinter.scrolledtext import ScrolledText
import tkinter as tk
import traceback
from tkinter import messagebox
from traceback import print_stack

import serial
from serial.serialwin32 import Serial
import threading
import struct

MAX_POINTS =100
class CommandUI:
    def bytes_to_hexstr(self, data):
        return " ".join(f"{b:02x}" for b in data).upper()

    def append_text(self, text):
        def _append():
            self.receive_textarea.configure(state='normal')
            self.receive_textarea.insert(tk.END, output_result)
            self.receive_textarea.see(tk.END)  # 自动滚动到底部
            self.receive_textarea.configure(state='disabled')

        line_num = int(self.receive_textarea.index('end-1c').split('.')[0])
        if line_num >= 40:
            # 删除前30行（从第1行到第30行）
            self.receive_textarea.configure(state='normal')
            self.receive_textarea.delete("1.0", tk.END)
            self.receive_textarea.configure(state='disabled')
        now = datetime.now()
        current_time = now.strftime("%H:%M:%S")  # 格式化输出
        output_result = "{}:{}\n".format(current_time, text)

        with open("app_log.txt", "a", encoding="utf-8") as f:
            f.write(output_result)

        self.receive_textarea.after(0, _append)
    def draw_column_data(self):
        data = deque([0.0] * MAX_POINTS, maxlen=MAX_POINTS)
        fig, ax = plt.subplots()
        line, = ax.plot(data, label="Serial Data")
        ax.set_ylim(-5, 5)  # 设置合适的Y轴范围
        ax.legend()
        ax.set_title("Real-time Serial Data Plot")


    def read_serial(self):
        uart_receive_status = 0
        cmd_receive_buffer = None

        while not self.is_thread_out:
            try:
                if self.com6.is_open:
                    if self.com6.in_waiting> 0:

                        if uart_receive_status == 0 :
                            start_flag:bytes  = self.com6.read(1)
                            if start_flag[0] == 0xFF:
                                uart_receive_status = 1
                        elif uart_receive_status == 1:
                            #2, 收到cmd，进入2
                            bytes_cmd_id = self.com6.read(1)
                            cmd_id = bytes_cmd_id[0]
                            try:
                                cmd_receive_buffer = BasicCmd.which_cmd(cmd_id)
                                cmd_receive_buffer.set_cmd_id(cmd_id)
                                uart_receive_status = 2
                            except:
                                print('error:no match cmdid {}'.format(cmd_id))
                                uart_receive_status = 0
                        elif uart_receive_status == 2 :
                            bytes_cmd_len = self.com6.read(1)
                            cmd_len = bytes_cmd_len[0]
                            # 收到len, 进入3
                            if cmd_len <= 30:
                                #有效
                                cmd_receive_buffer.set_cmd_len(cmd_len)
                                uart_receive_status = 3
                            else:

                                #长度过长，数据无效，丢弃
                                uart_receive_status = 0
                        elif uart_receive_status == 3:
                            rx_buffer = self.com6.read(cmd_receive_buffer.cmd_len)
                            cmd_receive_buffer.set_cmd_value(rx_buffer)
                            uart_receive_status = 0;
                            self.append_text(cmd_receive_buffer.generate_send_bytes())
                            if isinstance(cmd_receive_buffer, GimbalStatus):
                                self.pid_angle_data.append(cmd_receive_buffer.get_angle_volocity_pid_error())
                                self.pid_velocity_data.append(cmd_receive_buffer.get_velocity_pid_error())
                            elif isinstance(cmd_receive_buffer, HeadtrackEulerAngle):
                                self.pid_angle_data.append(cmd_receive_buffer.get_roll())
                                self.pid_velocity_data.append(cmd_receive_buffer.get_pitch())
                                self.z_data.append(cmd_receive_buffer.get_yaw())



                        else:
                            print("error: status is  not right :{}".format(uart_receive_status))

                        '''
                        data = self.com6.read(self.com6.in_waiting)  # 非阻塞读取
                        if data:
                            hex_str = self.bytes_to_hexstr(data)
                            print("receive hex:{}".format(hex_str))
                            # self.append_text(hex_str + "  ")
                        '''



                time.sleep(0.01)
            except Exception as e:
                print("error:{}".format(traceback.format_exc()))

    def __init__(self):

        self.is_thread_out = False
        self.com6:Serial = None
        # 创建主窗口
        self.root = tk.Tk()
        self.root.title("设备控制界面")

        # 设置窗口大小
        self.root.geometry("1800x850")

        # 端口号和设备编号输入框
        tk.Label(self.root, text="端口号").grid(row=0, column=0)
        self.text_port = tk.Entry(self.root)
        self.text_port.grid(row=0, column=1)
        self.text_port.insert(0, "COM7")  #

        tk.Label(self.root, text="baud").grid(row=0, column=2)
        self.text_baud = tk.Entry(self.root)
        self.text_baud.grid(row=0, column=3)
        self.text_baud.insert(0, "115200")  # 默认值


        tk.Label(self.root, text="设备编号").grid(row=0, column=4)
        self.text_deviceid = tk.Entry(self.root)
        self.text_deviceid.grid(row=0, column=5)
        self.text_deviceid.insert(0, "3")  # 默认值

        button_connect = tk.Button(self.root, text="连接", command=self.open_serial)
        button_connect.grid(row=0, column=6)


        # 选项按钮：设置云台基准角度 和 调整云台角度
        self.which_command  = tk.IntVar(value=1)
        tk.Radiobutton(self.root, text="设置云台基准角度", variable=self.which_command, value=1, anchor='w').grid(row=1, column=0, columnspan=2)
        tk.Radiobutton(self.root, text="调整云台角度", variable=self.which_command, value=2, anchor='w').grid(row=2, column=0, columnspan=2)

        # 电机编号和角度输入框
        tk.Label(self.root, text="电机编号").grid(row=1, column=2)
        self.cmd1_text_motor = tk.Entry(self.root)
        self.cmd1_text_motor.grid(row=1, column=3)
        self.cmd1_text_motor.insert(0, "0")  # 默认值

        tk.Label(self.root, text="角度(度)").grid(row=1, column=4)
        self.cmd1_text_angle = tk.Entry(self.root)
        self.cmd1_text_angle.grid(row=1, column=5)
        self.cmd1_text_angle.insert(0, "35")  # 默认值

        # 复制电机编号和角度输入框
        tk.Label(self.root, text="电机编号").grid(row=2, column=2)
        self.cmd2_text_motor = tk.Entry(self.root)
        self.cmd2_text_motor .grid(row=2, column=3)
        self.cmd2_text_motor .insert(0, "0")  # 默认值

        tk.Label(self.root, text="角度(度)").grid(row=2, column=4)
        self.cmd2_text_angle = tk.Entry(self.root)
        self.cmd2_text_angle.grid(row=2, column=5)
        self.cmd2_text_angle.insert(0, "35")  # 默认值

        # 发送按钮
        self.send_msg_button = tk.Button(self.root, text="发送", bg="blue", fg="white",command=self.send_cmd)
        self.send_msg_button.grid(row=3, column=2, columnspan=2)

        tk.Label(self.root, text="接收的消息(同时写入app_log.txt)").grid(row=4, column=1)

        self.receive_textarea = ScrolledText(self.root, wrap='none', state='disabled', height=20, width=160)
        self.receive_textarea.grid(row=4, column=2, columnspan=6)

        # 动态画图
        self.pid_angle_data = deque([0.0] * MAX_POINTS, maxlen=MAX_POINTS)
        self.pid_velocity_data = deque([0.0] * MAX_POINTS, maxlen=MAX_POINTS)
        self.z_data = deque([0.0] * MAX_POINTS, maxlen=MAX_POINTS)

        self.fig, self.ax = plt.subplots()
        self.angle_pid_error,    = self.ax.plot(self.pid_angle_data, label="angle pid error", color='blue')
        self.velocity_pid_error, = self.ax.plot(self.pid_velocity_data, label="velocity pid error", color='orange')
        self.z_fig,              = self.ax.plot(self.z_data, label="z", color='green')
        self.ax.set_ylim(-200, 200)  # 设置合适的Y轴范围
        self.ax.legend()
        self.ax.set_title("PID")
        # -----------------------------
        # 启动动画
        # -----------------------------
        ani = FuncAnimation(self.fig, self.update, interval=50, blit=True)

        canvas = FigureCanvasTkAgg(self.fig, master=self.root)  # 把 fig 放进 tkinter
        canvas.draw()
        canvas.get_tk_widget().grid(row=5, column=0, columnspan=4)


    def update(self, frame):
        try:

                self.angle_pid_error.set_ydata(self.pid_angle_data)
                self.velocity_pid_error.set_ydata(self.pid_velocity_data)
                self.z_fig.set_ydata(self.z_data)


                self.ax.relim()
                self.ax.autoscale_view()

        except (ValueError, UnicodeDecodeError):
            pass  # 忽略非法数据或解码错误
        return self.angle_pid_error,self.velocity_pid_error, self.z_fig




    def run(self):
        # 运行主循环
        self.root.mainloop()


    def open_serial(self):
        try:
            if self.com6:
                self.is_thread_out =  True
                time.sleep(2)
                self.com6.close()
                self.is_thread_out = False

            # 打开串口
            self.com6 = Serial(baudrate= int(self.text_baud.get())  )
            self.com6.port = self.text_port.get()

            self.com6.open()

            thread = threading.Thread(target=self.read_serial, daemon=True)  # 设置为守护线程
            thread.start()

            messagebox.showinfo("成功", "连接{}成功".format(self.text_port.get()))
        except Exception as e:
            messagebox.showerror("错误", f"打开串口失败: {e}")

    def send_cmd(self):
        if self.which_command.get() == 1 :
            self.send_CMD_INITAL_ZERO()
        elif self.which_command.get()== 2 :
            self.send_CMD_ADJUST_ANGLE()
    def send_serial_data(self, cmd_id:int, b_value:bytearray):
        device_id = int(self.text_deviceid.get())


        b1 = bytearray()

        # 起始符
        b1.append(0xFF)

        # cmd tag
        cmd_tag = struct.pack('B', cmd_id)
        b1.extend(cmd_tag)

        # len
        len_val = struct.pack('B', 2+len(b_value))
        b1.extend(len_val)

        # seq
        b1.append(1)

        # device_id
        dev_byte = struct.pack('B', device_id)
        b1.extend(dev_byte)

        # value
        b1.extend(b_value)
        print("串口发送数据 (hex):{}".format(b1.hex()))

        if self.com6.is_open:

            i = self.com6.write(b1)
            # print("send data len {}".format(i))
        else:
            messagebox.showerror("错误", "串口已经关闭，需要重启")
        # self.append_text('\r\n');


    # 发送数据函数
    def send_CMD_INITAL_ZERO(self):
        cmd_id = 0x2
        b1 = bytearray()
        b_motor_id = struct.pack('B', int(self.cmd1_text_motor.get()))
        b1.extend(b_motor_id)

        b_angle = struct.pack('f', float(self.cmd1_text_angle.get()))
        b1.extend(b_angle)
        self.send_serial_data(cmd_id, b1)

    def send_CMD_ADJUST_ANGLE(self):
        cmd_id = 0x3
        b1 = bytearray()
        b_motor_id = struct.pack('B', int(self.cmd2_text_motor.get()))
        b1.extend(b_motor_id)

        b_angle = struct.pack('f', float(self.cmd2_text_angle.get()))
        b1.extend(b_angle)
        self.send_serial_data(cmd_id, b1)






if __name__ == '__main__':
    a = CommandUI()
    a.run()